#include <WProgram.h>
#include "IAcomm.h"

#define DEBUG
#include "debug.h"

uint32_t IAcomm::SYNC_WAIT_TIME = SYNC_WAIT_TIME_DEF;
uint32_t IAcomm::CMD_RESPONSE_WAIT_TIME = CMD_RESPONSE_WAIT_TIME_DEF;

static const uint8_t MAX_SERIAL_BUFSIZE = 64;

IAcomm::IAcomm(uint8_t rxPin, uint8_t txPin): 
  methodId(0), cmdSent(0), cmdRcvd(0), numArgBytes(0), argCRC(0),
  peerMethodId(0),peerCmdRcvd(0),peerCmdSent(0), peerDataBytes(0),peerCRC(0),
  dataBuf(0),lenBuf(0),commStatus(NONE),
  peer(rxPin,txPin)
{
}

// Called when data received from peer.
uint8_t IAcomm::buf2fields() 
{
  if (hdrBuf[0] != 0xAA) return 0;

  peerMethodId = hdrBuf[1];
  peerCmdSent = hdrBuf[2];
  peerCmdRcvd = hdrBuf[3];
  peerDataBytes = (hdrBuf[4] << 8);
  peerDataBytes += hdrBuf[5];
  peerCRC = (hdrBuf[6] << 8);
  peerCRC += hdrBuf[7];
  
  return 1;
}

void IAcomm::setData(uint8_t method, uint16_t len, uint8_t* data, bool syncResponse)
{
  methodId = method;
  cmdSent++;
  numArgBytes = len;
  dataBuf = data;
  if (syncResponse) {
    argCRC = 1;
  } else {
    CRC();
  }
  fields2buf();  
}


void IAcomm::fields2buf()
{
  hdrBuf[0] = 0xAA;
  hdrBuf[1] = methodId;
  hdrBuf[2] = cmdSent;
  hdrBuf[3] = cmdRcvd;
  hdrBuf[4] = (numArgBytes >> 8);
  hdrBuf[5] = (numArgBytes & 0xFF);
  hdrBuf[6] = (argCRC >> 8);
  hdrBuf[7] = (argCRC & 0xFF);
}

void IAcomm::writeHdr2Net()
{
  peer.Print::write((uint8_t*)&hdrBuf,sizeof(hdrBuf));
}

// write chunks and wait for sync.
bool IAcomm::writeData2Net(int32_t deadline)
{
  uint16_t chunkSz = 0;
  lenBuf = 0;

  while (lenBuf < numArgBytes) {
    chunkSz = numArgBytes-lenBuf;
    if (chunkSz>MAX_SERIAL_BUFSIZE) chunkSz=MAX_SERIAL_BUFSIZE;
    peer.Print::write(dataBuf+lenBuf,chunkSz);
    lenBuf += chunkSz;
    if (!waitForSyncResponse(deadline)) {
      return false;
    }
  }
  return true;
}

bool IAcomm::send(uint8_t method, uint8_t* data, uint16_t length,
		  int32_t waitMs /* =CMD_RESPONSE_WAIT_TIME_DEF */)
{
  if (!methodId) {		// SYNC = 0, is private.
    _DPRINT("bad method.");
    return false;
  }
  commStatus = WRITING;

  int32_t deadline = waitMs;
  if (waitMs > 0) {
    deadline += millis();
  }

  setData(method,length,data);
  writeHdr2Net();
  if (!waitForSyncResponse(deadline)) {
    _DPRINT3("hdr method",method,HEX);
    return false;
  }

  if (length && !writeData2Net(deadline)) {
    _DPRINT3("data method",method,HEX);
    return false;
  }

  commStatus = NONE;
  return true;
}

void IAcomm::serialCheck()
{
  if (peer.overflow()) {
    _DPRINT("Serial:Overflow!");
  }
}

bool IAcomm::readHdr()
{
  uint8_t available;
  uint8_t needed = sizeof(hdrBuf);
  uint8_t* bufP = hdrBuf;

  for (uint8_t rep=5; rep; rep--) { // to account for link delays.
    while (available=peer.available()) {
      if (needed<=available) {
	while (needed--) *bufP++ = peer.read();

	if (!buf2fields()) {
	  _DPRINT("bad hdr");
	  return false;
	}
	sendSyncResponse();
	return true;
      }

      // else, needed > available
      needed -= available;
      while (available--) *bufP++ = peer.read();
    }
    delay(10);
  } 

  return false;
}

bool IAcomm::readValidReq()
{
  return (peer.available() && readHdr() && !methodIsSync());
}

bool IAcomm::recv(uint8_t& method, uint16_t& length,
		  int32_t waitMs /* =0 */)
{
  serialCheck();

  commStatus = READING;
  if (waitMs<0) {
    while (!readValidReq());
  } else if (!waitMs) {
    if (!readValidReq()) return false;
  } else {
    uint32_t deadline = millis() + waitMs;
    bool gotIt = false;
    while (!gotIt && millis() < deadline) {
      if (readValidReq()) {
	gotIt = true;
      }
    }
    if (!gotIt) return false;
  }

  method = peerMethodId;
  length = peerDataBytes;
  return true;
}


bool IAcomm::recvData(uint8_t* data,int32_t waitMs /* = 0 */)
{
  serialCheck();
  if (commStatus != READING) {
    _DPRINT3("commStatus",commStatus,HEX);
    return false;
  }

  if (!peerDataBytes) {
    _DPRINT("peerDataBytes=0");
    return true;
  }
    
  dataBuf = data;
  lenBuf = 0;
  if (waitMs<0) {
    while (lenBuf < peerDataBytes) {
      if (!readData(-1)) {
	return false;
      }
    }

  } else if (!waitMs) {
    if (!readData(0) || lenBuf < peerDataBytes) {
      return false;
    } 

  } else {
    // waitMs is some value other than 0 or -1.

    uint32_t deadline = millis() + waitMs;
    while (millis() < deadline && lenBuf < peerDataBytes) {
      if (!readData(deadline)) {
	return false;
      }
    }
    if (lenBuf < peerDataBytes) {
      return false;
    }

  }
  // successful cases fall thru'
  if (!checkCRC()) return false;

  commStatus = NONE;
  dataBuf = 0;

  return true;
}

// deadline is -1, 0 or some other value
bool IAcomm::readData(int32_t deadline)
{
  uint8_t available = 0;
  uint8_t *bufP = dataBuf + lenBuf;

  if (!deadline) {
    if (!(available=peer.available())) return false;
  } else if (deadline < 0) {
    while (!(available=peer.available()));
  } else {
    while (millis() < deadline) {
      if (available=peer.available()) break;
    }
    if (!available) return false;
  }

  while (available--) *bufP++ = peer.read();
  lenBuf += available;
  sendSyncResponse();
  return true;
}  


bool IAcomm::sync(int32_t waitMs /* =SYNCWAIT_TIME_DEF */)
{
  uint32_t cmdDeadline;
  bool ret = false;

  if (!waitMs) {
    sendSync();
    ret = waitForSyncResponse(waitMs);
  } else {
    uint32_t syncDeadline = millis() + waitMs;
    while (waitMs<0 ? true : millis() < syncDeadline ) {
      sendSync();
      cmdDeadline = millis() + CMD_RESPONSE_WAIT_TIME;
      if (waitForSyncResponse(cmdDeadline)) {
	ret = true;
	break;
      }
    }
  }

  commStatus = NONE;
  return ret;
}

bool IAcomm::waitForSyncResponse(int32_t deadline)
{
  if (!deadline) {
    if (readHdr() && methodIsSyncResponse()) {
      if (commStatus != WRITING ||
	  peerDataBytes == lenBuf) {
	return true;
      }
    }
    return false;

  } else {
    while ( deadline<0 ? true : millis() < deadline ) {
      if (readHdr() && methodIsSyncResponse()) {
	if (commStatus != WRITING) return true;
	if (peerDataBytes == lenBuf) return true;
      }
    }
    // we only get here if timed out.
    return false;
  }
}

void IAcomm::sendSync()
{
  setData(M_SYNC,0,0);
  commStatus = SYNC;
  writeHdr2Net();
}

void IAcomm::sendSyncResponse()
{
  setData(M_SYNC,lenBuf,0,true);
  writeHdr2Net();
}

void IAcomm::CRC()
{
  argCRC = 0;
  for (uint16_t i=0;i<numArgBytes; i++) argCRC += dataBuf[i];
}

bool IAcomm::checkCRC()
{
  uint16_t crc = 0;
  for (uint16_t i=0;i<peerDataBytes; i++) crc += dataBuf[i];  
  return (crc == peerCRC);
}
